#include "rtthread.h"
#include "bf0_hal.h"
#include "drv_io.h"
#include "stdio.h"
#include "string.h"
#include "drv_gpio.h"
#include "rtdevice.h"

#include "sensor_liteon_ltr303.h"
#include "st_lsm6dsl_sensor_v1.h"

#include "math.h"
#include "sensor_manager.h"

// 全局变量定义
static rt_device_t ltr303_dev, lsm6d_dev, lsm6d_gyro_dev, lsm6d_step_dev;

/**
  * @brief  初始化硬件IO（适配SF32LB52芯片GPIO规则）
  */
static void board_io_init(void)
{
    HAL_PIN_Set(PAD_PA40, I2C3_SCL, PIN_PULLUP, 1);
    HAL_PIN_Set(PAD_PA39, I2C3_SDA, PIN_PULLUP, 1);
}

/**
  * @brief  初始化所有传感器
  */
static void sensors_init(struct rt_sensor_config *cfg)
{
    cfg->intf.dev_name = "i2c3";
    rt_hw_ltr303_init("ltr303", cfg);

    cfg->intf.user_data = (void *)LSM6DSL_ADDR_DEFAULT;
    cfg->irq_pin.pin = RT_PIN_NONE;
    rt_hw_lsm6dsl_init("lsm6d", cfg);
}

/**
  * @brief  打开并配置传感器设备
  */
static void open_and_config_devices(void)
{
    ltr303_dev = rt_device_find("li_ltr303");
    if (ltr303_dev == RT_NULL)
        rt_kprintf("Can't find device:%s\n", "ltr303");
    else {
        rt_err_t ret = rt_device_open(ltr303_dev, RT_DEVICE_FLAG_RDONLY);
        if (ret != RT_EOK)
            rt_kprintf("open device failed! err: %d\n", ret);
        rt_device_control(ltr303_dev, RT_SENSOR_CTRL_SET_POWER, (void *)RT_SENSOR_POWER_NORMAL);
    }

    

    lsm6d_dev = rt_device_find("acce_lsm");
    lsm6d_gyro_dev = rt_device_find("gyro_lsm");
    lsm6d_step_dev = rt_device_find("step_lsm");
    if (lsm6d_dev == RT_NULL || lsm6d_gyro_dev == RT_NULL || lsm6d_step_dev == RT_NULL)
        rt_kprintf("Can't find device:%s\n", "lsm6d");
    else {
        rt_err_t ret = rt_device_open(lsm6d_dev, RT_DEVICE_FLAG_RDONLY);
        ret += rt_device_open(lsm6d_gyro_dev, RT_DEVICE_FLAG_RDONLY);
        ret += rt_device_open(lsm6d_step_dev, RT_DEVICE_FLAG_RDONLY);
        if (ret != RT_EOK)
            rt_kprintf("open device failed! err: %d\n", ret);
        rt_device_control(lsm6d_dev, RT_SENSOR_CTRL_SET_ODR, (void *)1660);
        rt_device_control(lsm6d_gyro_dev, RT_SENSOR_CTRL_SET_ODR, (void *)1660);
    }
}

/**
  * @brief  传感器初始化函数
  */
int sensor_init(void)
{
    rt_kprintf("Initializing sensors...\n");
    
    // 初始化硬件
    board_io_init();

    // 初始化传感器
    struct rt_sensor_config cfg = {0};
    sensors_init(&cfg);
    open_and_config_devices();
    
    rt_kprintf("Sensors initialized successfully!\n");
    return 0;
}

/**
  * @brief  获取环境光传感器数据
  */
int get_ltr303_data(int *light)
{
    if (ltr303_dev == RT_NULL) {
        return -1; // 设备未初始化
    }
    
    struct rt_sensor_data data;
    rt_size_t res = rt_device_read(ltr303_dev, 0, &data, 1);
    
    if (res != 1) {
        return -2; // 读取失败
    }
    
    *light = data.data.light;
    return 0;
}



/**
  * @brief  获取加速度计数据
  */
int get_lsm6d_acce_data(int *acce_x, int *acce_y, int *acce_z)
{
    if (lsm6d_dev == RT_NULL) {
        return -1; // 设备未初始化
    }
    
    struct rt_sensor_data data;
    rt_size_t res = rt_device_read(lsm6d_dev, 0, &data, 1);
    
    if (res != 1) {
        return -2; // 读取失败
    }
    
    *acce_x = data.data.acce.x;
    *acce_y = data.data.acce.y;
    *acce_z = data.data.acce.z;
    return 0;
}

/**
  * @brief  获取陀螺仪数据
  */
int get_lsm6d_gyro_data(int *gyro_x, int *gyro_y, int *gyro_z)
{
    if (lsm6d_gyro_dev == RT_NULL) {
        return -1; // 设备未初始化
    }
    
    struct rt_sensor_data data;
    rt_size_t res = rt_device_read(lsm6d_gyro_dev, 0, &data, 1);
    
    if (res != 1) {
        return -2; // 读取失败
    }
    
    *gyro_x = data.data.gyro.x;
    *gyro_y = data.data.gyro.y;
    *gyro_z = data.data.gyro.z;
    return 0;
}

/**
  * @brief  获取步数数据
  */
int get_lsm6d_step_data(int *step_count)
{
    if (lsm6d_step_dev == RT_NULL) {
        return -1; // 设备未初始化
    }
    
    struct rt_sensor_data data;
    rt_size_t res = rt_device_read(lsm6d_step_dev, 0, &data, 1);
    
    if (res != 1) {
        return -2; // 读取失败
    }
    
    *step_count = data.data.step;
    return 0;
}

/**
  * @brief  获取所有传感器数据
  */
int get_all_sensor_data(sensor_data_t *data)
{
    int ret = 0;
    
    // 获取环境光数据
    ret |= get_ltr303_data(&data->light);
    
    
    // 获取加速度数据
    ret |= get_lsm6d_acce_data(&data->acce_x, &data->acce_y, &data->acce_z);
    
    // 获取陀螺仪数据
    ret |= get_lsm6d_gyro_data(&data->gyro_x, &data->gyro_y, &data->gyro_z);
    
    // 获取步数数据
    ret |= get_lsm6d_step_data(&data->step_count);
    
    return ret;
}

/**
  * @brief  传感器数据采集循环（可选保留）
  */
static void sensors_loop(void)
{
    sensor_data_t data;
    
    if (get_all_sensor_data(&data) == 0) {
        // 打印所有传感器数据
        //rt_kprintf("Light: %d, Steps: %d\n", data.light, data.step_count);
        //rt_kprintf("Accel: X=%d, Y=%d, Z=%d\n", data.acce_x, data.acce_y, data.acce_z);
        //rt_kprintf("Gyro: X=%d, Y=%d, Z=%d\n", data.gyro_x, data.gyro_y, data.gyro_z);
        //rt_kprintf("----------------------------\n");
    } else {
        rt_kprintf("Failed to read sensor data\n");
    }
    
    rt_thread_mdelay(100);
}

